import launch
from launch.substitutions import Command, LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from nav2_common.launch import RewrittenYaml
import launch_ros
from launch.actions import TimerAction
import os

def generate_launch_description():
    pkg_share = launch_ros.substitutions.FindPackageShare(package='nmsl_bringup').find('nmsl_bringup')
    ################### user configure parameters for ros2 start ###################
    # xfer_format   = 0    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
    xfer_format   = 4    # 2023.10.11修改
    multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
    data_src      = 0    # 0-lidar, others-Invalid data src
    publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
    output_type   = 0
    frame_id      = 'base_link'
    lvx_file_path = '/home/livox/livox_test.lvx'
    cmdline_bd_code = 'livox0000000001'
    user_config_path = os.path.join(pkg_share, 'config', 'MID360_config.json')
    # rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud_ROS2.rviz')
    ################### user configure parameters for ros2 end #####################
    livox_ros2_params = [
        {"xfer_format": xfer_format},
        {"multi_topic": multi_topic},
        {"data_src": data_src},
        {"publish_freq": publish_freq},
        {"output_data_type": output_type},
        {"frame_id": frame_id},
        {"lvx_file_path": lvx_file_path},
        {"user_config_path": user_config_path},
        {"cmdline_input_bd_code": cmdline_bd_code}
    ]

    livox_driver = launch_ros.actions.Node(
        package='livox_ros_driver2',
        executable='livox_ros_driver2_node',
        name='livox_lidar_publisher',
        output='screen',
        parameters=livox_ros2_params
        )


    global_planner_cmd=launch_ros.actions.Node(
            package='global_planner',
            executable='global_planner',
            name='global_planner',
            parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')},
                        {'map_path': LaunchConfiguration('map_path')}],
            output='screen'
    )

    fast_lio2_node = launch_ros.actions.Node(
        package='fast_lio',
        executable='fastlio_mapping',
        parameters=[os.path.join(pkg_share, 'config', 'mid360.yaml'),
                    {'use_sim_time': LaunchConfiguration('use_sim_time')}],
        output='both'
    )

    apf_planner_cmd=launch_ros.actions.Node(
            package='local_planner',
            executable='ApfPlanner',
            name='ApfPlanner',
            parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
            output='screen',
            remappings=[('/mid360','segmentation/obstacle'),]
    )

    map_tf2_publish_cmd = launch_ros.actions.Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
            arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
            output='screen'
        )
    rviz_node = launch_ros.actions.Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            output='log',
            arguments=['-d', os.path.join(pkg_share, 'rviz', '111.rviz')],
        )
    ground_segmentation_cmd = launch_ros.actions.Node(
            package='linefit_ground_segmentation_ros',
            executable='ground_segmentation_node',
            output='log',
            parameters=[os.path.join(pkg_share, 'config', 'segmentation_params.yaml')])
    
    slam_cmd=launch_ros.actions.Node(
            package='slam_gmapping', 
            executable='slam_gmapping', 
            output='log',)
    
    simple_move_cmd=launch_ros.actions.Node(
            package='simple_move', 
            executable='nmsl1', 
            output='log',)

    point_clout_convert =launch_ros.actions.Node(
            package="pointcloud_to_laserscan",
            executable="pointcloud_to_laserscan_node",
            remappings=[('/cloud_in','/segmentation/obstacle')],
            parameters=[{"min_height":0.0},
                        {"max_height":0.6},
                        {"scan_time":0.1},
                        {"range_min":0.3},
                        {"range_max":10.0},
                        {"angle_increment":3.14159/900},
                        {"target_frame":"base_link"}] # 0.2deg
            )

    return launch.LaunchDescription([
        launch.actions.DeclareLaunchArgument(name='map_path', default_value='empty.pgm',
                                            description='Absolute path to map file'),
        launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='False',
                                            description='Flag to enable use_sim_time'),

        TimerAction(period=3.0, actions=[rviz_node]),
        livox_driver,
        fast_lio2_node,
        ground_segmentation_cmd,
        map_tf2_publish_cmd,
        global_planner_cmd,
        apf_planner_cmd,
        slam_cmd,
        point_clout_convert,
        simple_move_cmd

        
    ])
